#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/mman.h>
#include "math.h"
#include <alchemy/task.h> 
#include <alchemy/timer.h> 
#include <rtdm/rtdm.h>
//#include <iostream>
//#include<stdio.h>
/****************************************************************************/
#include "ecrt.h"
static int run = 1;
static unsigned int cycle_ns = 1000000; 
/****************************************************************************/
 RT_TASK my_task; 

/*Application Parameters*/

#define PROFILE_VELOCITY            8 /*Operation mode for 0x6060:0*/
#define pi 3.1415926
/*****************************************************************************/
#define DC_FILTER_CNT          1024
static uint64_t dc_start_time_ns = 0LL;
static uint64_t dc_time_ns = 0;

static uint8_t  dc_started = 0;
static int32_t  dc_diff_ns = 0;
static int32_t  prev_dc_diff_ns = 0;
static int64_t  dc_diff_total_ns = 0LL;
static int64_t  dc_delta_total_ns = 0LL;
static int      dc_filter_idx = 0;
static int64_t  dc_adjust_ns;

static int64_t  system_time_base = 0LL;
static uint64_t wakeup_time = 0LL;
static uint64_t overruns = 0LL;
#define sign(val) \
    ({ typeof (val) _val = (val); \
    ((_val > 0) - (_val < 0)); })

/*EtherCAT*/
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {};

static ec_slave_config_t *sc;
static ec_slave_config_state_t sc_state = {};

/****************************************************************************/

/*Process Data*/
static uint8_t *domain1_pd = NULL;
#define slaves 1 //slave number

#define DM3E         0                       /*EtherCAT address on the bus*/

#define VID_PID           0x00000083,0x00000005  /*Vendor ID, product code*/

/*Offsets for PDO entries*/
static struct{
    unsigned int operation_mode[slaves];
    unsigned int ctrl_word[slaves];
    unsigned int target_velocity[slaves];
    unsigned int target_position[slaves];
    unsigned int status_word[slaves];
    unsigned int current_velocity[slaves];
    unsigned int current_position[slaves];
}offset;

/*const static ec_pdo_entry_reg_t domain1_regs[] = {
    {DM3E, VID_PID, 0x6040, 0, &offset.ctrl_word},
    {DM3E, VID_PID, 0x6060, 0, &offset.operation_mode },
    {DM3E, VID_PID, 0x60FF, 0, &offset.target_velocity},
    {DM3E, VID_PID, 0x607A, 0, &offset.target_position},
    {DM3E, VID_PID, 0x6041, 0, &offset.status_word},
    {DM3E, VID_PID, 0x606C, 0, &offset.current_velocity},
    {DM3E, VID_PID, 0x6064, 0, &offset.current_position},
    {}
};*/
  static ec_pdo_entry_reg_t domain1_regs[slaves*7];

/***************************************************************************/
/*Config PDOs*/
static ec_pdo_entry_info_t device_pdo_entries[] = {
    /*RxPdo 0x1600*/
    {0x6040, 0x00, 16},
    {0x6060, 0x00, 8 }, 
    {0x60FF, 0x00, 32},
    {0x607A, 0x00, 32},
    /*TxPdo 0x1A00*/
    {0x6041, 0x00, 16},
    {0x606C, 0x00, 32},
    {0x6064, 0x00, 32}
};

static ec_pdo_info_t device_pdos[] = {
    //RxPdo
    {0x1600, 4, device_pdo_entries + 0 },
    //TxPdo
    {0x1A00, 3, device_pdo_entries + 4}
};

static ec_sync_info_t device_syncs[] = {
    { 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE },
    { 1, EC_DIR_INPUT, 0, NULL, EC_WD_ENABLE },
    { 2, EC_DIR_OUTPUT, 1, device_pdos + 0, EC_WD_ENABLE },
    { 3, EC_DIR_INPUT, 1, device_pdos + 1, EC_WD_ENABLE},
    { 0xFF}
};
uint64_t system_time_ns(void)
{
   RTIME time = rt_timer_read();

      
    if ((system_time_base>0)&&((uint64_t)system_time_base>time)) 
    {
        
        rt_printf("%s() error: system_time_base greater than"
                " system time (system_time_base: %lld, time: %llu\n",
                __func__, system_time_base, time);
        return time;
    }
    else 
    {
        return time - system_time_base;
    }
}


//获取不同周期下apptime的和参考时间ref_time的误差
void sync_distributed_clocks(void)
{

    uint32_t ref_time=0;
    uint64_t prev_app_time =dc_time_ns;


    static uint32_t last_ref_time=0;
    static uint64_t u64_reftime=0;

    static uint64_t i__=0;
    static uint64_t last_diff=0;
    static int32_t period_max_time=0;
    static  int32_t  period_min_time=0;
       dc_time_ns = system_time_ns();
      // set master time in nano-seconds
     // get reference clock time to synchronize master cycle
     ecrt_master_reference_clock_time(master, &ref_time);
     
    
     //rt_printf("prev_app_time %u\n",(uint32_t)prev_app_time);
     dc_diff_ns =(uint32_t)prev_app_time-ref_time;
     if(ref_time<last_ref_time)
     {
      u64_reftime=u64_reftime+(ref_time+4294967296-last_ref_time);

     }
     else
      {
        u64_reftime=u64_reftime+(ref_time-last_ref_time);
         
      }
     uint32_t diff=u64_reftime%1000000;
     last_ref_time=ref_time; 
     
                   if(i__<900)
                      {
                       period_min_time=diff;
                      } 

                    
                    if(i__>10000)
		      
		      {
			     if(diff>period_max_time)
			     {
				 period_max_time=diff;
		
			     }
			    
			     if((diff<period_min_time)&&( period_min_time>0))
			     {
				 period_min_time=diff;
			    	 				
			     }
                      
                      }
           rt_printf("period_max_time %lld\n",period_max_time);
		       rt_printf("period_min_time %lld\n",period_min_time);
           rt_printf("\n");
           rt_printf("diff   %u\n",diff);
           rt_printf("\n");
           rt_printf("u64_reftime %llu\n",u64_reftime);
           rt_printf("\n");       
                	
			last_diff=diff;      






     
    
    
    
     i__++;
     // call to sync slaves to ref slave
     ecrt_master_sync_slave_clocks(master);         
}
SRTIME system2count(uint64_t time)
{   
    SRTIME ret;

      if ((system_time_base < 0) &&
           ((uint64_t) (-system_time_base) > time)) {
          rt_printf("%s() error: system_time_base less than"
         " system time (system_time_base: %lld, time: %llu\n",
                __func__, system_time_base, time);
          ret = time;
         }  
       else
         {
          ret = time+system_time_base;
         }
    return rt_timer_ns2ticks(ret);
    
     
}

void update_master_clock(void)
{

    // calc drift (via un-normalised time diff)
    int32_t delta = dc_diff_ns - prev_dc_diff_ns;
    prev_dc_diff_ns = dc_diff_ns;
     
    dc_diff_ns =((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2);
    dc_diff_total_ns += dc_diff_ns;
    dc_delta_total_ns += delta;
   
   
  
        // add to totals
        dc_diff_total_ns += dc_diff_ns;
        dc_delta_total_ns += delta;
        dc_filter_idx++;

        if (dc_filter_idx >= DC_FILTER_CNT) 
      {
            // add rounded delta average
            dc_adjust_ns +=((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT);

            // and add adjustment for general diff (to pull in drift)
            dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT);
            
            // limit crazy numbers (0.1% of std cycle time)
            if (dc_adjust_ns < -1000) 
            {
               dc_adjust_ns = -1000;
            }
            if (dc_adjust_ns > 1000) 
            {
               dc_adjust_ns =  1000;
            }
            // reset
            dc_diff_total_ns = 0LL;
            dc_delta_total_ns = 0LL;
            dc_filter_idx = 0;
      }
   
        // add cycles adjustment to time base (including a spot adjustment)
        system_time_base += dc_adjust_ns + sign(dc_diff_ns);
        
        //rt_printf("system_time_base%lld\n",system_time_base);
  
}


void rt_check_domain_state(void)
{
    ec_domain_state_t ds = {};

	ecrt_domain_state(domain1, &ds);

    if (ds.working_counter != domain1_state.working_counter) {
        rt_printf("Domain1: WC %u.\n", ds.working_counter);
    }

    if (ds.wc_state != domain1_state.wc_state) {
        rt_printf("Domain1: State %u.\n", ds.wc_state);
    }

    domain1_state = ds;
}

void rt_check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding) {
        rt_printf("%u slave(s).\n", ms.slaves_responding);
    }

    if (ms.al_states != master_state.al_states) {
        rt_printf("AL states: 0x%02X.\n", ms.al_states);
    }

    if (ms.link_up != master_state.link_up) {
        rt_printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }

    master_state = ms;
}

/****************************************************************************/

void check_slave_config_states(void)
{
    ec_slave_config_state_t s;
    ecrt_slave_config_state(sc, &s);
    if (s.al_state != sc_state.al_state)
    {
        printf("slave: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_state.online)
    {
        printf("slave: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_state.operational)
    {
        printf("slave: %soperational.\n", s.operational ? "" : "Not ");
    }
    sc_state = s;
}
void wait_period(void)
{
    while (1)
    {
        SRTIME wakeup_count = system2count(wakeup_time);
        RTIME current_count = rt_timer_read();

        if ((wakeup_count < current_count)|| (wakeup_count > current_count + (50 * cycle_ns))) 
        {
            rt_printf("%s(): unexpected wake time!\n", __func__);
            exit(1);
        }

        switch (rt_task_sleep_until(wakeup_count)) 
        {
            case EPERM:
                rt_printf("rt_sleep_until(): RTE_UNBLKD\n");
                continue;

            case ETIMEDOUT:
                rt_printf("rt_sleep_until(): RTE_TMROVRN\n");
                overruns++;

                if (overruns % 100 == 0)
              {
                    // in case wake time is broken ensure other processes get
                    // some time slice (and error messages can get displayed)
                    rt_task_sleep(cycle_ns / 100);
               }
                break;

            default:
                break;
        }

        // done if we got to here
        break;
    }

    ecrt_master_application_time(master, wakeup_time);


    // calc next wake time (in sys time)
    wakeup_time += cycle_ns;
    //rt_printf("wakeup_time=%ld\n",wakeup_time);









}
/*******************************************************************************/
void enable (void)
{
    
             static int64_t enable=0;
             int32_t read_position=0;
              int32_t position_add=0;
               if(enable<50000)
             {
                for(int i=0;i<slaves;i++)
                {
                      EC_WRITE_U16(domain1_pd + offset.ctrl_word[i], 0x80 );
                }
                  

                 if(enable>10000)
                 {
                 
		         for(int i=0;i<slaves;i++)
		        {
		              EC_WRITE_S8(domain1_pd + offset.operation_mode[i], PROFILE_VELOCITY);
		        }
		         
		               
                 }    
                                      
                if(enable>20000)
                {
		          for(int i=0;i<slaves;i++)
		        {
		               EC_WRITE_U16(domain1_pd + offset.ctrl_word[i], 0x0006 ); 
		        }
                              
                }
                if(enable>30000)
                    {       
                            
                         for(int i=0;i<slaves;i++)
		        {
		             EC_WRITE_U16(domain1_pd + offset.ctrl_word[i], 0x0007 );  
		        }
                            
                     
                    }
                if(enable>40000)
                {
                      for(int i=0;i<slaves;i++)
		        {
		                EC_WRITE_U16(domain1_pd + offset.ctrl_word[i], 0x000f ); 
                               read_position=EC_READ_S32(domain1_pd + offset.current_position[i]);   
		        }
                      
                                     
                }
               
                                         
               enable++;
               rt_printf("%lld\n",enable++);
              // read_position=EC_READ_S32(domain1_pd + offset.current_position); 
             }   
             else
              {
                   

		                       static long double t=0;
			    int32_t position=(int32_t )(3000000*sin(t));
				  t=t+0.0004;
                          for(int i=0;i<slaves;i++)
		        {
		               EC_WRITE_S32(domain1_pd + offset.target_position[i],position);  
		        }
                      
			    
		       
	                           		      		    
              }     
}
void signal_handler(int sig)
{
    run = 0;
}

void my_task_proc(void *cookie)
{
   
  wakeup_time = system_time_ns() + 10 * cycle_ns;
   
  while(run)
  {  
      
     
      
     
           
     
      
      wait_period();
      uint64_t time1=rt_timer_read();
      ecrt_master_receive(master);

      
      ecrt_domain_process(domain1);
      
      enable ();
      
        
       ecrt_domain_queue(domain1);
        
       sync_distributed_clocks();

       
       
      
       ecrt_master_send(master); 
       
     uint64_t time2=rt_timer_read();
    static uint64_t time3=0;
     if((time2-time1)>time3)

  {
       time3=(time2-time1);
  }
       rt_printf(" time3   %llu\n",time3);
       update_master_clock();
      
    
     
     
    
    
  }

}
/* Create a real-time task */ 


int main(int argc, char **argv)
{
  
   for(int i=0;i<slaves;i++)
   {    
        
        domain1_regs[7*i].alias=0;
        domain1_regs[7*i].position=i;
        domain1_regs[7*i].vendor_id=0x00000083;
        domain1_regs[7*i].product_code=0x00000005;
        domain1_regs[7*i].index=0x6040;  
        domain1_regs[7*i].subindex=0;  
        domain1_regs[7*i].offset=&offset.ctrl_word[i];                
        
        domain1_regs[7*i+1].alias=0;
        domain1_regs[7*i+1].position=i;
        domain1_regs[7*i+1].vendor_id=0x00000083;
        domain1_regs[7*i+1].product_code=0x00000005;
        domain1_regs[7*i+1].index=0x6060;  
        domain1_regs[7*i+1].subindex=0;  
        domain1_regs[7*i+1].offset=&offset.operation_mode[i]; 
        
        domain1_regs[7*i+2].alias=0;
        domain1_regs[7*i+2].position=i;
        domain1_regs[7*i+2].vendor_id=0x00000083;
        domain1_regs[7*i+2].product_code=0x00000005;
        domain1_regs[7*i+2].index=0x60ff;  
        domain1_regs[7*i+2].subindex=0;  
        domain1_regs[7*i+2].offset=&offset.target_velocity[i]; 
                
        domain1_regs[7*i+3].alias=0;
        domain1_regs[7*i+3].position=i;
        domain1_regs[7*i+3].vendor_id=0x00000083;
        domain1_regs[7*i+3].product_code=0x00000005;
        domain1_regs[7*i+3].index=0x607A;  
        domain1_regs[7*i+3].subindex=0;  
        domain1_regs[7*i+3].offset=&offset.target_position[i]; 
        
        domain1_regs[7*i+4].alias=0;
        domain1_regs[7*i+4].position=i;
        domain1_regs[7*i+4].vendor_id=0x00000083;
        domain1_regs[7*i+4].product_code=0x00000005;
        domain1_regs[7*i+4].index=0x6041;  
        domain1_regs[7*i+4].subindex=0;  
        domain1_regs[7*i+4].offset=&offset.status_word[i]; 
        
        domain1_regs[7*i+5].alias=0;
        domain1_regs[7*i+5].position=i;
        domain1_regs[7*i+5].vendor_id=0x00000083;
        domain1_regs[7*i+5].product_code=0x00000005;
        domain1_regs[7*i+5].index=0x606c;  
        domain1_regs[7*i+5].subindex=0;  
        domain1_regs[7*i+5].offset=&offset.current_velocity[i]; 
        
        
        domain1_regs[7*i+6].alias=0;
        domain1_regs[7*i+6].position=i;
        domain1_regs[7*i+6].vendor_id=0x00000083;
        domain1_regs[7*i+6].product_code=0x00000005;
        domain1_regs[7*i+6].index=0x6064;  
        domain1_regs[7*i+6].subindex=0;  
        domain1_regs[7*i+6].offset=&offset.current_position[i];
       
                     
   }

    int ret;
 
    printf("Requesting master...\n");
    master = ecrt_request_master(0);
    if (!master)
    {
        exit(EXIT_FAILURE);
    }
    
    domain1 = ecrt_master_create_domain(master);
    if (!domain1)
    {
        exit(EXIT_FAILURE);
    }
  for(int i=0;i<slaves;i++)
    {
	    if (!(sc = ecrt_master_slave_config(master, DM3E,i, VID_PID)))
	    {
		fprintf(stderr, "Failed to get slave configuration for slave!\n");
		 exit(EXIT_FAILURE);
	    }
	    printf("Configuring PDOs...\n");
    
	     if (ecrt_slave_config_pdos(sc, EC_END, device_syncs))
	    {
	       fprintf(stderr, "Failed to configure slave PDOs!\n");
	       exit(EXIT_FAILURE);
	    }
	    else
	    {
		printf("*Success to configuring slave PDOs*\n");
	    }
	    if(i==0)
	    {
	      ret = ecrt_master_select_reference_clock(master,sc);
	    } 
	ecrt_slave_config_dc(sc,0x0300,cycle_ns,150000,0,0);
    }
   
         
    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) 
    {
        fprintf(stderr, "PDO entry registration failed!\n");
        exit(EXIT_FAILURE);
    }
    
    //ecrt_slave_config_dc(sc,0x0300,cycle_ns,500000,0,0);
    dc_start_time_ns = system_time_ns();
    dc_time_ns = dc_start_time_ns;

    ret = ecrt_master_select_reference_clock(master, NULL);
    if (ret < 0) 
    {
         fprintf(stderr, "Failed to select reference clock: %s\n",
         strerror(-ret));
         return ret;
    }
    printf("Activating master...\n");
    if (ecrt_master_activate(master))
    {
        exit(EXIT_FAILURE);
    }
    else
    {
        printf("*Master activated*\n");
    }
    if (!(domain1_pd = ecrt_domain_data(domain1)))
    {
        exit(EXIT_FAILURE);
    }

    rt_printf("*It's working now*\n");
     
     ret = rt_task_create(&my_task, "my_task", 0, 99, 0);
     if (ret < 0)
    {
        fprintf(stderr, "Failed to create task: %s\n", strerror(-ret));
        return -1;
    }
        int cpu_id =7;                // 需要绑定的cpu
            cpu_set_t mask;                // cpu核的位掩码
            CPU_ZERO(&mask);               // 置空
            CPU_SET(cpu_id, &mask);        // 将需要绑定的cpu号设置在mask中 
   int a= rt_task_set_affinity(&my_task,&mask); 
  if(a!=0)
   {
       exit(1);
   }
    	
    rt_printf("Starting my_task...\n");
    
    ret = rt_task_start(&my_task, &my_task_proc, NULL);
    if (ret < 0) 
    {
        fprintf(stderr, "Failed to start task: %s\n", strerror(-ret));
        return -1;
    }

	
       pause();
       ecrt_release_master(master);
       ret=rt_task_delete(&my_task);	
             /* Disable paging for this program'smemory. */ 
             
             /* Create a real-time task */ 
  
    return 0;
}



